#include "RaiseAngleSet.h"

RaiseAngleSet::RaiseAngleSet(double angle) {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	Requires(shooterElevator);
	m_angle = angle;
}

// Called just before this Command runs the first time
void RaiseAngleSet::Initialize() {
	
}

// Called repeatedly when this Command is scheduled to run
void RaiseAngleSet::Execute() {
	shooterElevator->Raise(1.0);
}

// Make this return true when this Command no longer needs to run execute()
bool RaiseAngleSet::IsFinished() {
	double angle = shooterElevator->getPotAngle();
//	cout << "angle: " << angle <<" m_angle: " << m_angle << endl;
	if(angle >= m_angle){
		shooterElevator->elevatorMotor->Set(0);
		cout << "stop1" << endl;
		return true;
	}
	return false;
}

// Called once after isFinished returns true
void RaiseAngleSet::End() {
	shooterElevator->elevatorMotor->Set(0);
	cout << "stop2" << endl;
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void RaiseAngleSet::Interrupted() {
	shooterElevator->elevatorMotor->Set(0);
	cout << "stop3" << endl;
}
